#include "DashUI.h"
#include "WPILib.h"
#define MIN_SPEED 0.2
#define WAIT_TIME 0.05
#define TURN_SPEED 0.25
#define TRACK_SPEED 0.25
#define WINCH_SPEED 0.75
#define RELAY_SPEED 3 //every 3 iterations, the relay will move if rotated

class RobotDemo : public SimpleRobot
{

	Joystick driverStick, armStick; 
	CANJaguar leftMotor, rightMotor, winch;
	Victor arm, miniBotMotor;
	DigitalInput splitDirection, lightsensor1, lightsensor2, lightsensor3;
	Gyro gyro;
	RobotDrive robot;
	Relay armRotate;
	float heart;
	int relayCounter;

public:

	RobotDemo(void):

		driverStick(1),
		armStick(2),
		leftMotor(12),
		rightMotor(10),
		winch(13),
		arm(1),
		miniBotMotor(2),
		splitDirection(1),
		lightsensor1(4), lightsensor2(5), lightsensor3(6),
		gyro(2),
		robot(leftMotor,rightMotor),
		armRotate(1)
		{
		robot.SetExpiration(0.1);
		heart = 0.0;
		relayCounter = 0;
		}


	void Autonomous(void)
	{
		robot.SetSafetyEnabled(false);
		bool foundEnd = false;
		while (!foundEnd)
		{
			int result = trackLine(TRACK_SPEED);
			Wait(WAIT_TIME);
			robot.Drive(0.0, 0.0);
			if (result == 1)
			{
				/*
				winch.Set(1);
				Wait(1.5);
				winch.Set
				(0);
				armRotate.Set(Relay::kForward);
				Wait(0.5);
				armRotate.Set(Relay::kOff);
				arm.Set(-1);
				Wait(0.25);
				 */
				foundEnd = true;
			}
			else if (result == -1)
			{
				robot.Drive(TURN_SPEED, -TURN_SPEED);
				Wait(WAIT_TIME);
			}
		}

		//         result value    |         what happened
		//---------------------------------------------------------
		//              0          | robot is progressing         
		//				1          | robot hit the end
		//              -1         | terrible things have happened
	}

	int trackLine(float speed)
	{
		//trackLine only uses the left thumbstick
		if (!lightsensor1.Get() && lightsensor2.Get() && !lightsensor3.Get())
		{
			if (splitDirection.Get())
				robot.TankDrive(speed-speed/5.0, speed);
			else 
				robot.TankDrive(speed, speed-speed/5.0);
			Wait(WAIT_TIME);
			return 0;
		}

		if (!lightsensor1.Get() && !lightsensor2.Get() && !lightsensor3.Get())
			return 1;
		if (lightsensor1.Get() && lightsensor2.Get() && lightsensor3.Get())
			return -1;

		if (!lightsensor1.Get()) 
		{
			robot.TankDrive(speed-speed/5.0, speed);                   //turn right
			Wait(WAIT_TIME);
			robot.TankDrive(speed, speed-speed/5.0);                   //straighten out
			Wait(WAIT_TIME);
		}
		if (!lightsensor3.Get()) 
		{
			robot.TankDrive(speed, speed-speed/5.0);                   //turn left
			Wait(WAIT_TIME);
			robot.TankDrive(speed-speed/5.0, speed);                   //straighten out
			Wait(WAIT_TIME);
		}
		if (!lightsensor2.Get()) robot.TankDrive(speed, speed); //go straight
		return 0;
	}



	void OperatorControl(void)
	{
		robot.SetSafetyEnabled(false);
		//AxisCamera &camera = AxisCamera::GetInstance();
		DriverStationLCD *dsl = DriverStationLCD::GetInstance();
		//camera.WriteResolution(AxisCamera::kResolution_640x480);
		//camera.WriteExposureControl(AxisCamera::kExposure_FlickerFree60Hz);
		//camera.WriteWhiteBalance(AxisCamera::kWhiteBalance_Automatic);
		//camera.WriteCompression(0);

		while (IsOperatorControl())
		{
			relayCounter++;
			sendIOData();
			sendOtherData(heart, (driverStick.GetRawAxis(2)+driverStick.GetRawAxis(5))/-2.0, gyro.GetAngle());
			dsl->Clear();
			dsl->Printf(DriverStationLCD::kUser_Line1, 1, "%f", gyro.GetAngle());
			dsl->Printf(DriverStationLCD::kUser_Line2, 1, "%d %d %d", lightsensor1.Get(), lightsensor2.Get(), lightsensor3.Get());
			dsl->UpdateLCD();

			if (driverStick.GetRawButton(3)){
				int r = trackLine(driverStick.GetRawAxis(2));
				if (r==-1) robot.TankDrive(driverStick.GetRawAxis(2), driverStick.GetRawAxis(2));
			}
			else
				if (driverStick.GetRawAxis(3)<-0.75)
					robot.TankDrive(driverStick.GetRawAxis(2), driverStick.GetRawAxis(2));
				else
					robot.TankDrive(driverStick.GetRawAxis(2), driverStick.GetRawAxis(5));


			if (armStick.GetRawAxis(2)>MIN_SPEED) 
				armRotate.Set(Relay::kForward);
			else if (armStick.GetRawAxis(2)<-MIN_SPEED) {
				if (relayCounter % RELAY_SPEED == 0) 
					armRotate.Set(Relay::kReverse);
			}
			else 
				armRotate.Set(Relay::kOff);


			if(armStick.GetRawAxis(3)<0)
				winch.Set(armStick.GetRawAxis(3)*WINCH_SPEED/2);
			else
				winch.Set(armStick.GetRawAxis(3)*WINCH_SPEED);

			if (armStick.GetRawButton(1))
				arm.Set(-0.5);
			else if (armStick.GetRawButton(2))
				arm.Set(0.5);
			else
				arm.Set(0.0);

			if (armStick.GetRawButton(3))
			{
				miniBotMotor.Set(1);
				Wait(2);
				miniBotMotor.Set(0);
			}

			Wait(0.005);	// wait for a motor update time
		}
	}
};

START_ROBOT_CLASS(RobotDemo);
